#ifndef VMC_H
#define VMC_H

#include "BSP.h"
#include "myMath.h"

#define L1 150
#define L2 300
#define L3 330

#define MAX_L (L1 + L2) * 0.9 // 最长腿长
#define MIN_L (L2 - L1) * 1.1 // 最短腿长
#define MAX_A _PI             // 最大角度
#define MIN_A 0               // 最小角度

typedef struct
{
    float sita1, sita2;
    float torque[3], force[3];
    float jacobi33[9], invJacobi33[9];
    float jacobi22[4], invJacobi22[4];
} VMC_t;

void state_solution(VMC_t *in, float sita1, float sita2, float *x, float *z);
void state_inv_solution(VMC_t *in, float x, float z, float *sita1, float *sita2);
void cal_jacobi(VMC_t *in);
uint8_t cal_invJacobi(VMC_t *in);
void cal_omega(VMC_t *in, float Vx, float Vz, float *alphaW, float *betaW);
void cal_velcity(VMC_t *in, float alphaW, float betaW, float *Vx, float *Vz);
void cal_torque(VMC_t *in, float forceX, float forceZ, float *torqueX, float *torqueZ);

#define Xr 0
#define Zr 1
#define Yr 2

#endif
